% normalized outputs
addpath_output()
% name list 9 person
name  = {'Charles','Fred','Martin','Ram','Selina',... % 1st experiment
    'lily','po','ryan','vic'};  % 2nd experiment

% CONSTANT
% the number of subjects from the 1st experiment
exp1st = 5;
% the index of subjects use the right leg as swing leg during the step
rIndex = [6,7,9];
% experimental sampling frequency
fs = 480;
% model = 'NAO';
% model = 'HM';
model  = 'AMBER';
time_rec = zeros(9,1);
for index = 1:length(name) % 5
    
    % get human data, with breaking pts for each step
    humanData = getData(name{1,index});
    
    %     sum_weight  = sum_weight+humanData.weight; % for average weight
    % breaking pts for each step
    StartPt = humanData.sns_brk(2);
    EndPt = humanData.sns_brk(3);
    
    % position
    if index <= exp1st
        [xpos,ypos,zpos] = getPosCartesian(humanData); % subjects from experiment 1st
    else
        [xpos,ypos,zpos] = getPosCartesianNew(humanData); % subjects from experiment 2st
    end
    
    %%%%%%%%%%%  time  %%%%%%%%%%%%%%
    % scale time
    % from [c:d]
    ns_time_pre = (0:length(StartPt:EndPt)-1)'/fs;
    ns_ret = ns_time_pre;
    xpara_time = 1/ns_ret(end); % simply sacled time to 1
    ns_time = ns_ret*xpara_time;
    %%%% record time %%%%%%%%
    time_rec(index) = ns_ret(end);
    
    
    
    %%%%%%%%%%%% calculate human joint angles %%%%%%%%%%%%%%%%%
    if ismember(index,rIndex)
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,2);
    else
        angle = Knee2D_angle(xpos,ypos,zpos,humanData.sns_brk,1);
    end
    
    %%%%%%%%%%% use joint angles to calculate the outputs %%%%%%%%%%
    q = [angle.sa;angle.sknee;angle.theta3;angle.theta4;angle.nsknee];
    
    outputs = cal_robot_para_18(q,model);
    
    % 1. hip position
    [ave_dataHP,ave_timeHP] = BeamData(ns_time',outputs.hippos);
    all_outputs. ave_dataHP_s(index,:) =ave_dataHP;
    
    %     plot(ave_dataHP); hold on;
    
    
    % 2. linearized hip position
    [ave_dataHPL,ave_timeHPL] = BeamData(ns_time,outputs.delta_hippos);
    all_outputs.ave_dataHPL_s(index,:) =ave_dataHPL;
    %     plot(ave_dataHPL); hold on;
    
    
    % 3. non-stance slope
    [ave_dataNSslope,ave_timeNSslope] = BeamData(ns_time,outputs.nsslope);
    all_outputs.ave_dataNSslope_s(index,:) =ave_dataNSslope;
    %     plot(ave_dataNSslope); hold on;
    
    % 4. linearized non-stance slope
    [ave_dataNSslopeL,ave_timeNSslopeL] = BeamData(ns_time,outputs.delta_nsslope);
    all_outputs.ave_dataNSslopeL_s(index,:) =ave_dataNSslopeL;
    
    %     5. stance COM slope
    [ave_dataSCOM,ave_timeSCOM] = BeamData(ns_time, outputs.stCOM);
    all_outputs.ave_dataSCOM_s(index,:) = ave_dataSCOM;
    
    %     6. linearized stance COM slope
    [ave_dataSCOML,ave_timeSCOML] = BeamData(ns_time, outputs.delta_stCOM);
    all_outputs.ave_dataSCOML_s(index,:) = ave_dataSCOML;
    
    
    
    %%%%%%%%%%%%%%%%%%% calculate the angle outputs  %%%%%%%%%%%%%%%%%%%%%
    % 1. hip angle
    [ave_dataHip,ave_timeHip] = BeamData(ns_time,angle.hip);
    ave_dataHip_s(index,:) =ave_dataHip;
    
    % 2. stance knee
    [ave_dataSK,ave_timeSK] = BeamData(ns_time,angle.sknee);
    ave_dataSK_s(index,:) = ave_dataSK;
    
    % 3. non-stance knee
    [ave_dataNSK,ave_timeNSK] = BeamData(ns_time,angle.nsknee);
    ave_dataNSK_s(index,:) = ave_dataNSK;
    
    % 4. torso hip angle
    [ave_dataTH,ave_timeTH] = BeamData(ns_time,angle.torso_hip);
    ave_dataTH_s(index,:) = ave_dataTH;
    
    
end
ave_time = ave_timeHP;

%% calculate the mean value and save
%%%%%%%%%%%% angles %%%%%%%%%%%%%%%%%%%%%%%%%
% 1. hip angle
[x_meanHip,upperBHip,lowerBHip] = meanValue(ave_dataHip_s);

% 2. stance knee
[x_meanSK,upperBSK,lowerBSK] = meanValue(ave_dataSK_s);
%     plot(ave_dataSK); hold on;

% 3. non-stance knee
[x_meanNSK,upperBNSK,lowerBNSK] = meanValue(ave_dataNSK_s);

% 4. torso hip angle
[x_meanTH,upperBTH,lowerBTH]=meanValue(ave_dataTH_s);

%%%%%%%%%%%%%%%% slopes and position %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[Xmean,XupperB,XlowerB] = cal_mean_outputs(all_outputs);
%% save the data
 ns_time_mean = mean(time_rec)*ave_time'; % averaged time
 
 data_name = 'mean_data';
 folder_name = ['data_' model '_18'];
 save_outputs(Xmean,ns_time_mean,data_name,folder_name);
%%%%%%%%%%%% angles %%%%%%%%%%%%%%%%
ns_time = ns_time_mean;
 % 1. hip angle
hip_angle =x_meanHip';
save(['./', folder_name, '/',data_name,'_hipAngle.mat'],'hip_angle','ns_time')

 % 3. non-stance knee angle
ns_knee = x_meanNSK';
save(['./', folder_name, '/', data_name,'_nsknee.mat'],'ns_knee','ns_time');

% 4. stance knee angle
s_knee = x_meanSK';
save(['./', folder_name, '/', data_name,'_sknee.mat'],'s_knee','ns_time');

% 5. torso angle
torso_angle =x_meanTH';
save(['./', folder_name, '/',data_name, '_TorsoHipAngle.mat'],'torso_angle','ns_time');
%% convert
% hip position
% ave_timeHP;
upperBHP = XupperB.upperBHP;
lowerBHP = XlowerB.lowerBHP;
x_meanHP = Xmean.x_meanHP;
% linearized hip position
% ave_timeHPL;
upperBHPL = XupperB.upperBHPL;
lowerBHPL = XlowerB.lowerBHPL;
x_meanHPL = Xmean.x_meanHPL;
% non-stance slope
upperBNSL = XupperB.upperBNSL;
lowerBNSL = XlowerB.lowerBNSL;
x_meanNSL = Xmean.x_meanNSL;
% linearized non-stance slope
% ave_timeNSslopeL = ave_timeNSslopeL;
upperBLNS = XupperB.upperBLNS;
lowerBLNS = XlowerB.lowerBLNS;
x_meanLNS = Xmean.x_meanLNS;
% stance COM slope
upperBSCOM = XupperB.upperBSCOM;
lowerBSCOM = XlowerB.lowerBSCOM;
x_meanSCOM = Xmean.x_meanSCOM;
% linearized stance COM slope
upperBSCOML = XupperB.upperBSCOML;
lowerBSCOML = XlowerB.lowerBSCOML;
x_meanSCOML = Xmean.x_meanSCOML;
%%
ave_timeSA = ave_time;
    meanValuePlot(ave_timeSA,upperBHP,lowerBHP,x_meanHP,'HipPos.eps',...
        'Position(m)',1);
   
    meanValuePlot(ave_timeSA,upperBHPL,lowerBHPL,x_meanHPL,'HipPosLinear.eps',...
        'Position(m)',2);

    meanValuePlot(ave_timeSA,upperBNSL,lowerBNSL,x_meanNSL,'nsSlope.eps',...
        'Slope',3)

    meanValuePlot(ave_timeSA,upperBLNS,lowerBLNS,x_meanLNS,'nsSlopeLiear.eps',...
        'Slope',4)

    meanValuePlot(ave_timeSA,upperBSCOM,lowerBSCOM,x_meanSCOM,'stCOM.eps',...
        'Slope',9);

        meanValuePlot(ave_timeSA,upperBSCOML,lowerBSCOML,x_meanSCOML,'stCOMLinear.eps',...
        'Slope',10);
    close all;